#include <p24FJ256GA106.h>
#include <stdio.h>

#define RP_SB_IN   RPOR5bits.RP11R
#define RP_SB_UIT  RPOR6bits.RP12R

#define LAT_SB_IN   LATDbits.LATD0
#define LAT_SB_UIT  LATDbits.LATD11


/*ADC 1kHz pinX
 *RP11&RP12
 *RC13&RC14
 */

unsigned char I2C_OutputData[1];	
 
void InitPWM(void)//pwm in compare mode
{	
  // BB pwm in en uit output
  TRISCbits.TRISC13 = 0;
  TRISCbits.TRISC14 = 0;
  // BB pwm porten laag maken
  LATCbits.LATC13 = 0;
  LATCbits.LATC14 = 0;


  // SB pwm in en uit output
  TRISDbits.TRISD0 = 0;
  TRISDbits.TRISD11 = 0;
  // SB pwm porten laag maken
  LAT_SB_IN = 0;
  LAT_SB_UIT = 0;

  // OC 1 wordt voor SB gebruikt, in en uit.
  // OC 2 wordt voor BB gebruikt, in en uit.
  // De pinnen van OC1 en OC 2 worden gewoon geswitched voor in of uit
	//RPORXbits.RPXXR=19;			//oc2
  
  OC1CON1 = 0; /* It is a good practice to clear off the control bits initially */
  OC1CON2 = 0;
  OC1CON1bits.OCTSEL = 0x07; 	/* This selects the peripheral clock(Fcy!) as the clock input to the OC module '0000000000000111' 	*/

  OC1R = 1000;
  OC1RS = 3950;
  T1CON=0;					//stelt timer1 in, snelheid is Fosc/2
		
  PR1=3950; 					//maximale waarde die timer1 kan bereiken
  OC1CON2bits.SYNCSEL = 0x0B;/* TMR1 is the sync source */
  OC1CON1bits.OCM = 5; /* This selects the Continuous Pulse mode*/
  OC1CON1bits.OCM = 0b101;	/*Double Compare Continuous Pulse mode: Initialize OCx pin low, toggle OCx als r &rs matchen als het klopt gaat timer nu pas lopen		*/

 /*
  if(ReadSBData(I2C_OutputData)!=1) 
  {
    while(1)
 	{
  	  KnipperLedRood();
	  putsU1("Error loading EEPROM");
	  putsU2("Error loading EEPROM");
	  putsU3("Error loading EEPROM");
    }  
  }*/
}

// Deze variable zorgen voor een soort van anti dender
int iSB_In = 0;
int iSB_Uit = 0;

void PWM_SB_In(void)
{
  if(iSB_In > 5) {
    LedGroen(1);
    LedRood(0);
	
	if(RP_SB_IN!=18)
	{
	  putU2("ATSB_IN."); 		// Laat control board weten dat SB IN gaat
	  // Verander PWM pin
	  LAT_SB_IN = 0;		    // voor test bordje
      RP_SB_IN  = 18;			// OC 1 op RP11
	  RP_SB_UIT = 0;			// RP12 NULL
  	  LAT_SB_UIT = 1;		    // voor test bordje
	}
	// start timer
	T1CONbits.TON	= 1; 
  } 
  iSB_In++;
  iSB_Uit = 0;
}

void PWM_SB_Uit(void)
{
  if(iSB_Uit > 5) {
  
    LedGroen(0);
    LedRood(1);

	if(RP_SB_UIT!=18)
	{
	  putU2("ATSB_UIT."); 		// Laat control board weten dat SB IN gaat
	  // Verander PWM pin
      LAT_SB_UIT = 0;		    // voor test bordje
      RP_SB_IN  = 0;			// RP11 NULL
	  RP_SB_UIT = 18;			// OC 1 op RP12
	  LAT_SB_IN = 1;		    // voor test bordje
	}
	// start timer
	T1CONbits.TON	= 1; 
  } 
  iSB_Uit++;
  iSB_In = 0;
}	

void PWM_SB_Clear(void)
{
  LedGroen(0);
  LedRood(0);

  RP_SB_IN  = 0;			// RP11 NULL
  RP_SB_UIT = 0;			// RP12 NULL
  
  // Stop PWM timer
  T1CONbits.TON	= 0; 
}
int Rem = 0;
void PWM_Rem(int NieuweRem)
{
  Rem = NieuweRem;
  
  if(Rem==1)
  {
  // SB Uit
  LedGroen(0);
  LedRood(1);

  if(RP_SB_UIT!=18)
  {
	putU2("ATSB_UIT."); 		// Laat control board weten dat SB IN gaat
	// Verander PWM pin
	LAT_SB_UIT = 0;		    // voor test bordje
	RP_SB_IN  = 0;			// RP11 NULL
	RP_SB_UIT = 18;			// OC 1 op RP12
    LAT_SB_IN = 1;		    // voor test bordje
  }
  // start timer
  T1CONbits.TON	= 1; 

  // BB Uit
  }
}

#define HYSTERESE 10

void UpdateSB(int UpdateNaarMy)
{

  if(Rem==1) {
	UpdateNaarMy = 0;
  }

  // Als het stuursignaal onder 511 ligt,
  // dan wordt er gestuurt naar SB
  if(UpdateNaarMy <= 511)
  {
    // ADC_Stuur moet gelijk aan ADC_SB worden
	// Als ADC_Stuur==ADC_SB dan staat de klep goed
	if(UpdateNaarMy > (GetADC_SB(511)+HYSTERESE)) 
  	{
	  PWM_SB_Uit();
	} 
    else if(UpdateNaarMy < (GetADC_SB(511)-HYSTERESE)) 
  	{
	  PWM_SB_In();
	} 
    else 
    {
	  PWM_SB_Clear();
	}
  } 
  else 
  {
    // Als Update waarde hoger is dan 511, dan wordt BB aangestuurd
	// Dus SB moet volledig ingetrokken worden
	if(GetADC_SB(1023)!=0) 
    {
	  PWM_SB_In();
	} 
    else 
    { // Als SB al volledig is ingetrokken dan stoppen met aansturen
	  PWM_SB_Clear();
 	}
  }
}